/**
* @file BumperPlanner.h
* @author
* @brief 障碍物碰撞规划
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include <cstdint>
#include <vector>
#include "data_struct/robot/BumperData.h"
#include "lib/contour.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
class BumperPlanner
{
public:
    /**
     * @brief 碰撞传感器响应的构造函数
     * @return
     */
    BumperPlanner() {};

    /**
     * @brief 碰撞传感器响应的运行函数
     * @return
     */
    void run(double &linear_vel, double &angular_vel);

    /**
     * @brief 控制机器人后退转向函数
     * @return
     */
    void BackAndTurn(double &linear_vel, double &angular_vel, double angle = 0);

    void Turn(double &linear_vel, double &angular_vel, double dst_angle);

    bool Check(double distance_front = ROBOT_DIAMETER, double distance_left = ROBOT_RADIUS);

    bool CollisionWarning(double distance_front = ROBOT_DIAMETER, double distance_left = ROBOT_RADIUS);

    int lastWarningFlag()
    { return LAST_WARNING_FLAG; };

public:
    bool finished = true; //沿悬崖动作是否结束的标记

private:
    double time_now = 0;   //用于进行时间赋值的标记，如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
    double time_now2 = 0;
    double global_goal_angle;
    const double turn_error1 = 0.78539815;  //45deg
    const double turn_error2 = 0.087266461;  //5deg
    double dst_angle = 0;
    int WARNING_FLAG = 0, LAST_WARNING_FLAG = 0; // 0 没有发现， 1 发现非直线，2 发现直线
    bool finished_turn = true;
};

//static
extern BumperPlanner bumper_planner;
} // namespace behavior_controller